#include <stdarg.h>
#include <stdint.h>
#include "common.h"

extern int _prf(int (*func)(), void *dest, const char *format, va_list vargs);
extern int _prf_locked(int (*func)(), void *dest, char *format, va_list vargs);
static void __rt_io_unlock();
static void __rt_io_lock();

int strcmp(const char *s1, const char *s2)
{
    while (*s1 != '\0' && *s1 == *s2) {
        s1++;
        s2++;
    }

    return (*(unsigned char *) s1) - (*(unsigned char *) s2);
}

int strncmp(const char *s1, const char *s2, size_t n)
{
  if (n == 0)
    return 0;

    while (n-- != 0 && *s1 == *s2) {
        if (n == 0 || *s1 == '\0') {
            break;
        }
        s1++;
        s2++;
    }

    return (*(unsigned char *) s1) - (*(unsigned char *) s2);
}

size_t strlen(const char *str)
{
  const char *start = str;

    while (*str) {
        str++;
    }
    return str - start;
}

int memcmp(const void *m1, const void *m2, size_t n)
{
    unsigned char *s1 = (unsigned char *) m1;
    unsigned char *s2 = (unsigned char *) m2;

    while (n--) {
        if (*s1 != *s2) {
            return *s1 - *s2;
        }
        s1++;
        s2++;
    }

    return 0;
}
// Don't delete it. If you delete it, the consequences will be very serious.
// gcc user the -o3 compilation option. The compiled program is err, which makes the dsp program unable to execute under -o3.
void *memset(void *m, int c, size_t n) __attribute__((optimize("O1")));

void *memset(void *m, int c, size_t n)
{
    char *s = (char *)m;
    while (n--) {
        *s++ = (char) c;
    }

    return m;
}

void *memcpy(void *dst0, const void *src0, size_t len0) {
    void *save = dst0;

    char src_aligned = (((size_t) src0) & 3) == 0;
    char dst_aligned = (((size_t) dst0) & 3) == 0;
    char copy_full_words = (len0 & 3) == 0;

    if (src_aligned && dst_aligned && copy_full_words) {
        // all accesses are aligned => can copy full words
        uint32_t *dst = (uint32_t *) dst0;
        uint32_t *src = (uint32_t *) src0;

        while (len0) {
            *dst++ = *src++;
            len0 -= 4;
        }
    }
    else {
        // copy bytewise
        char *dst = (char *) dst0;
        char *src = (char *) src0;

        while (len0) {
            *dst++ = *src++;
            len0--;
        }
    }

    return save;
}

void *memmove(void *d, const void *s, size_t n)
{
    char *dest = d;
    const char *src  = s;

    if ((size_t) (dest - src) < n) {
        /*
        * The <src> buffer overlaps with the start of the <dest> buffer.
        * Copy backwards to prevent the premature corruption of <src>.
        */
        while (n > 0) {
            n--;
            dest[n] = src[n];
        }
    }
    else {
        /* It is safe to perform a forward-copy */
        while (n > 0) {
          *dest = *src;
          dest++;
          src++;
          n--;
        }
    }

    return d;
}

char *strcpy(char *d, const char *s)
{
    char *dest = d;
    while (*s != '\0') {
        *d = *s;
        d++;
        s++;
    }
    *d = '\0';
    return dest;
}

char *strcat(char *dest, const char *src)
{
    strcpy(dest + strlen(dest), src);
    return dest;
}

char *strchr(const char *s, int c)
{
    char tmp = (char) c;

    while ((*s != tmp) && (*s != '\0')) {
        s++;
    }

    return (*s == tmp) ? (char *) s : NULL;
}

static inline void tfp_putc(void *data, char c)
{
#if defined(PUTC_UART)
    putc_uart(c);
#else
    putc_stdout(c);
#endif
}

#pragma pack (4)
static volatile uint32_t io_lock = 0;
#pragma pack ()

static inline void __rt_io_lock()
{
#ifdef ENABLE_T1
  mtx_lock(&io_lock);
#endif
}

static inline void __rt_io_unlock()
{
#ifdef ENABLE_T1
  mtx_unlock(&io_lock);
#endif
}

int puts(const char *s) {

    __rt_io_lock();

    char c;
    do {
        c = *s;
        if (c == 0) {
            tfp_putc(NULL, '\n');
            break;
        }
        tfp_putc(NULL, c);
        s++;
    } while(1);

    __rt_io_unlock();

    return 0;
}

int fputc_locked(int c, FILE *stream)
{
    tfp_putc(NULL, c);

    return c;
}

int fputc(int c, FILE *stream)
{
    int err;

    __rt_io_lock();

    err = fputc_locked(c, stream);

    __rt_io_unlock();

    return err;
}

int _prf_locked(int (*func)(), void *dest, char *format, va_list vargs)
{
    int err;

    __rt_io_lock();

    err =  _prf(func, dest, format, vargs);

    __rt_io_unlock();

    return err;
}

